#include "DebugTask.h"
#include "Gimbal.h"
#include "Chassis.h"
#include "Motor_control.h"
#include "Shoot.h"
#include "bsp_can.h"
#include "MonitorTask.h"
#include "Mode_SW.h"
#include "remoteCtrl.h"
#include "nuc_interface.h"
#include "INS_task.h"
#include "bmi088driver.h"
#include <stdlib.h>
/********************************** Extern variables ****************************************/
extern motorData_t s_gimbal_yaw_motor;
extern motorData_t s_gimbal_pit_motor;
extern chassisMove_t s_chassisMove;
extern motorData_t s_trans_motor;
extern motorData_t s_fric_l_motor, s_fric_r_motor;
extern uint8_t chassis_txBuff[8], chasPower_txBuff[8];
extern uint8_t RobotId, RobotLevel;
extern OFFLINE_COUNT_STRUCT    Fps, calFps;
extern visInf_t s_visionInform;
extern PID_AbsoluteType s_gimbal_pit_vision_pos_pid, s_gimbal_pit_vision_spd_pid, s_gimbal_pit_motor_spd_pid, s_gimbal_pit_motor_pos_pid;
extern PID_AbsoluteType s_gimbal_yaw_vision_spd_pid, s_gimbal_yaw_vision_pos_pid, s_gimbal_pit_encodmotor_spd_pid, s_gimbal_pit_encodmotor_pos_pid;
extern PID_AbsoluteType s_chassisMove_pos_pid;
extern PID_AbsoluteType s_trans_motor_spd_pid, s_trans_motor_pos_pid;
extern PID_AbsoluteType s_gimbal_yaw_motor_spd_pid;
extern PID_AbsoluteType s_gimbal_yaw_motor_pos_pid;
extern motorData_t s_chassis_motor[4];
extern PC_DATA pcData;
extern float dtaX,visX,visImu,relV,pre;
extern float yawSpd_buff[10];
extern uint8_t capExpect_txBuff[8], chassis_txBuff[8], chasPower_txBuff[8];
extern PID_AbsoluteType s_gimbal_yaw_encodmotor_spd_pid,s_gimbal_yaw_encodmotor_pos_pid;
extern roboSetting_t roboSet;
extern remoteCtrl_t s_rcData;
extern bmi088_real_data_t bmi088_real_data;
extern PC_DATA pcData;
void ANOTech_send_data(float buff1,float buff2,float buff3);

void StartDebugTask(void const * argument)
{
  /* USER CODE BEGIN StartDebugTask */
  /* Infinite loop */
  for(;;)
  {
    osDelay(100);
		
    /****************Mode detection****************/
//     printf("%d %d %d %d %d\r\n", s_rcData.rc.s[1], s_rcData.rc.s[0], roboSet.e_aimingMode, roboSet.e_chassisMode, roboSet.e_shootMode);

    /**************Referee Trans*********************/
    // printf("%d %d %d %d %f %f %d %d\r\n", \
    //                           RobotId, \
    //                           RobotLevel, \
    //                           s_trans_motor.coolingheat, \
    //                           s_trans_motor.coolingheat_limit, \
    //                           s_chassisMove.chassis_power, \
    //                           s_chassisMove.chassis_power_limit, \
    //                           s_chassisMove.chassis_power_buffer, \
    //                           s_fric_l_motor.SpeedTarget);
    /****************Shooting Obj**************/
    // printf("%d\r\n", roboSet.shootOnce);
    // printf("%d %d\r\n",s_fric_l_motor.esc_back_speed, s_fric_r_motor.esc_back_speed);
    // printf("%f, %f, %f, %f\r\n", s_trans_motor_spd_pid.errNow, \
    //                                           s_trans_motor_spd_pid.ctrOut, \
    //                                           s_trans_motor_pos_pid.errNow, \
    //                                           s_trans_motor_pos_pid.ctrOut);
    // printf("%d %d %d %d %d\r\n", \
    //                         (int16_t)s_trans_motor.turnMove, \
    //                         (int16_t)s_trans_motor.position_angle, \
    //                         (int16_t)s_trans_motor.turnMove-(int16_t)s_trans_motor.position_angle, \
    //                         s_trans_motor.esc_back_position, \
    //                         s_trans_motor.circle_num);

    /**************chassis Obj****************/
    // printf("%f %f\r\n",s_chassisMove.gyro_angle, s_chassisMove.gyro_speed);
//     printf("%f \r\n", s_chassisMove_pos_pid.errNow);
//			 printf("%d \r\n", s_chassis_motor[0].SpeedTarget);
//     printf("%d %d %d %d\r\n", \
//                             s_chassis_motor[0].esc_back_speed, \
//                             s_chassis_motor[1].esc_back_speed, \
//                             s_chassis_motor[2].esc_back_speed, \
//                             s_chassis_motor[3].esc_back_speed);
    /****************YawMotor Obj*************/
    // printf("%f\r\n", s_gimbal_yaw_motor.gyro_error);
//    printf("%f, %f, %f, %f, %f\r\n", \
//                                     s_gimbal_yaw_encodmotor_spd_pid.errNow, \
//                                     s_gimbal_yaw_encodmotor_spd_pid.ctrOut, \
//                                     s_gimbal_yaw_encodmotor_pos_pid.errNow, \
//                                     s_gimbal_yaw_encodmotor_pos_pid.ctrOut, \
                                     s_gimbal_yaw_motor.gyro_speed);
//    printf("%f, %f, %f, %f, %f\r\n", \
//                                     s_gimbal_yaw_motor_spd_pid.errNow, \
//                                     s_gimbal_yaw_motor_spd_pid.ctrOut, \
//                                     s_gimbal_yaw_motor_pos_pid.errNow, \
//                                     s_gimbal_yaw_motor_pos_pid.ctrOut, \
//                                     s_gimbal_yaw_motor.gyro_angle);
//     printf("%f, %f, %f, %f\r\n",s_gimbal_yaw_motor.yawEncodTarget, \
//                                     s_gimbal_yaw_motor.gyro_angle, \
//                                     s_gimbal_yaw_motor.position_angle, \
//                                     s_gimbal_yaw_motor.gyro_speed);

    /******************PitMotor Obj*******************/
//    printf("%f\r\n", s_gimbal_pit_motor.gyro_speed);
    // printf("%f,%f,%f\r\n", s_gimbal_pit_motor.gyro_angle, s_gimbal_pit_motor.gyro_speed, s_gimbal_pit_motor.PositionTarget);
//     printf("%f, %f, %f, %f, %f, %f, %f\r\n", \
//                                     s_gimbal_pit_encodmotor_spd_pid.errNow, \
//                                     s_gimbal_pit_encodmotor_spd_pid.ctrOut, \
//                                     s_gimbal_pit_encodmotor_pos_pid.errNow, \
//                                     s_gimbal_pit_encodmotor_pos_pid.ctrOut, \
//                               (float)s_gimbal_pit_motor.pitEncodTarget, \
//                               (float)s_gimbal_pit_motor.position_angle, \
//                               s_gimbal_pit_motor.gyro_speed);
//		printf("position = %.4f\r\n",s_gimbal_pit_motor.position_angle); 

    /**************Vision Obj*****************/
//		printf("%f %f %f\r\n", pcData.x.data, pcData.y.data, pcData.z.data);
    // ANOTech_send_data(visImu, visX, relV);
//		printf("%f\r\n", s_visionInform.yawPre);
//     printf("%f,%f,%f\r\n",visX, visImu, s_visionInform.yawRelV );
//     printf("%f %f %f %f %f %f\r\n", s_visionInform.yawPos, \
//                             s_gimbal_yaw_vision_pos_pid.errNow, \
//                             s_gimbal_yaw_vision_pos_pid.ctrOut,\
//                             s_gimbal_yaw_vision_spd_pid.errNow, \
//                             s_gimbal_yaw_vision_spd_pid.ctrOut,\
//                             s_gimbal_yaw_motor.gyro_speed);
//    printf("%f,%f,%f,%f,%f\r\n",\
//																			s_gimbal_pit_vision_pos_pid.errNow, \
//																			s_gimbal_pit_vision_pos_pid.ctrOut, \
//                                      s_gimbal_pit_vision_spd_pid.errNow, \
//                                      s_gimbal_pit_vision_spd_pid.ctrOut, \
//                                      s_gimbal_pit_motor.visPitTarget);

    /**************Super Cap************/
    // printf("%f %f %f %f %f %d\r\n", s_chassisMove.capInV, \
    //                           s_chassisMove.capVol, \
    //                           s_chassisMove.chassis_power_limit, \
    //                           s_chassisMove.cap_expect, \
    //                           s_chassisMove.chassis_power, \
    //                           s_chassisMove.chassis_power_buffer
    //                           );

    /****************Fps****************/
//     printf("%d %d %d %d %d %d %d %d %d %d %d %d\r\n",
//                                                 calFps.chassis[0], \
//                                                 calFps.chassis[1], \
//                                                 calFps.chassis[2], \
//                                                 calFps.chassis[3], \
//                                                 calFps.yaw,        \
//                                                 calFps.pitch_r,    \
//                                                 calFps.pitch_l,    \
//                                                 calFps.trans,      \
//                                                 calFps.fric_l,     \
//                                                 calFps.fric_r,     \
//                                                 calFps.pc,         \
//                                                 calFps.judge);
/********************Print Pic*************************/
//	printf("%f,%f\r\n", s_visionInform.yawRelV, s_visionInform.yawPre);
//	printf("%f,%f\r\n",s_visionInform.pitPos, s_visionInform.pitchCenter);
//  printf("%f,%f,%f\r\n", s_gimbal_yaw_motor.gyro_angle, s_gimbal_yaw_motor.gyro_speed, s_gimbal_pit_motor.gyro_speed);
    // Fps.usart_print++;
  }
  /* USER CODE END StartDebugTask */
}
//static void usart1_send_char(uint8_t c)
//{
//	while((USART1->SR&0X40)==0);
//	USART1->DR=c;
//}

//static void usart1_niming_report(uint8_t fun,uint8_t*data,uint8_t len)
//{
//	uint8_t send_buf[32];
//	uint8_t i;
//	if(len>28)return;    //最多28字节数据
//	send_buf[len+3]=0;  //校验数置零
//	send_buf[0]=0X88;   //帧头
//	send_buf[1]=fun;    //功能字
//	send_buf[2]=len;    //数据长度
//	for(i=0;i<len;i++)send_buf[3+i]=data[i];         //复制数据
//	for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];    //计算校验和
//	for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);   //发送数据到串口6
//}

//void ANOTech_send_data(float buff1,float buff2,float buff3)
//{
//	uint8_t tbuf[16];
//	unsigned char *p;
//	p=(unsigned char *)&buff1;
//	tbuf[0]=(unsigned char)(*(p+3));
//	tbuf[1]=(unsigned char)(*(p+2));
//	tbuf[2]=(unsigned char)(*(p+1));
//	tbuf[3]=(unsigned char)(*(p+0));

//	p=(unsigned char *)&buff2;
//	tbuf[4]=(unsigned char)(*(p+3));
//	tbuf[5]=(unsigned char)(*(p+2));
//	tbuf[6]=(unsigned char)(*(p+1));
//	tbuf[7]=(unsigned char)(*(p+0));

//	p=(unsigned char *)&buff3;
//	tbuf[8]=(unsigned char)(*(p+3));
//	tbuf[9]=(unsigned char)(*(p+2));
//	tbuf[10]=(unsigned char)(*(p+1));
//	tbuf[11]=(unsigned char)(*(p+0));

//	usart1_niming_report(0XA2,tbuf,28);//自定义帧,0XA2
//}

void ModePrint(void)
{
  printf("left%d %d\r\n", s_rcData.rc.s[1], s_rcData.rc.s[0]);
}